#ifndef _STATEESTIMATOR_H_
#define _STATEESTIMATOR_H_

#include "params.h"
#include "global.h"
#include <Eigen/Dense>


using namespace Eigen;
using namespace std;

/*
    状态估计器
        所有过程变量均作为成员变量存储在类中，调用对应get方法后赋值
*/

class StateEstimator
{
private:
    double t_; // 当前时间

    // IMU
    Vector3d a_com_B_;     // B系质心加速度
    Vector3d a_com_O_;     // O系质心加速度
    Vector3d omega_OB_B_;  // B系躯干角速度
    Vector3d omega_OB_O_;  // O系躯干角速度
    Matrix3d omega_OBX_B_; // B系躯干角速度张量(2.51)

    Vector3d p_i_B_;     // 本体系下i足足底位置(2.47)
    Vector3d p_dot_i_B_; // 本体系下i足足底速度(2.49)

    Matrix3d R_B_O_;         // B->O躯干旋转矩阵(通过以下矩阵计算得出)
    Matrix3d R_B_imu_;       // B->imu旋转矩阵(常值)
    Matrix3d R_imu_imu0_;    // IMU原始旋转矩阵(任何时刻直接从IMU读取)
    Matrix3d R_imu0_O_;      // imu0->O旋转矩阵(常值)
    Matrix3d R_t0_imu_imu0_; // 初始时刻IMU旋转矩阵(IMU启动时读出)

    Vector<double, 18> x_k_; // k时刻状态变量[p_com_O, v_com_O, p_foot_O](18*1)

public:
    StateEstimator();
    ~StateEstimator();

    void setIMUdata(const Matrix3d &R_imu_imu0);
    void setEncoderdata(const Vector3d &q_j_i, const Vector3d &q_dot_j_i);

    Matrix3d getR_B_O(); // 计算并返回躯干旋转矩阵

    Vector3d geta_com_B(const Vector3d &omega_original);    // 计算并返回质心加速度
    Vector3d geta_com_O();                                  //(2.43)
    Vector3d getomega_OB_B(const Vector3d &omega_original); // 计算并返回躯干角速度
    Vector3d getomega_OB_O();                               //(2.43)

    Matrix3d getomega_OBX_B();

    Vector3d getp_i_B(int i, const Vector3d &q_j_i); // 计算本体系下i足足底位置(2.47)
    Vector3d getp_dot_i_B(int i, const Vector3d &q_j_i, const Vector3d &q_dot_j_i);
    Vector<double, 18> getx_k(const Vector3d &q_j_i, const Vector3d &q_dot_j_i);
};

#endif